#include "../../include/api/moveApi.h"
#include "../../include/tool/configManage.h"
#include "../../include/device/chassisDevice.h"

#include <iostream>
#include <thread>
#include <unistd.h> // 引入 usleep()


C_chassisDevice *p_chassisDevice;

C_moveApi::C_moveApi(void)
{
}

C_moveApi::~C_moveApi(void)
{
}



/********************************************************************
 * @author: tianxiaohuahua
 * @date: 23.8
 * @description: initTask 初始化移动底盘通信模块
 * @input: 底盘的通信设置 
 * @return: void 
*******************************************************************/
int32_t C_moveApi::initTask(CONFIG_SINGCHIP &confif_singchip)
{
    task_name   = confif_singchip.config_task.task_name;
    task_period = confif_singchip.config_task.task_period;
    task_topic  = confif_singchip.config_task.task_topic;
    spdlog::info("..............................................................");
    spdlog::info("C_moveApi::initTask");
    task_name   = confif_singchip.config_task.task_name;
    task_period = confif_singchip.config_task.task_period;
    task_topic  = confif_singchip.config_task.task_topic;
    spdlog::info("C_moveApi task_name:'{}'", task_name);
    spdlog::info("C_moveApi task_period:'{}'", task_period);
    spdlog::info("C_moveApi task_topic:'{}'", task_topic);
    spdlog::info("C_moveApi nband:          '{}'",confif_singchip.config_serial_hardware.baud);
    spdlog::info("C_moveApi ndata_bits:     '{}'",confif_singchip.config_serial_hardware.data_bits);
    spdlog::info("C_moveApi nserial_device: '{}'",confif_singchip.config_serial_hardware.serial_device );
    spdlog::info("C_moveApi nstop_bits:     '{}'",confif_singchip.config_serial_hardware.stop_bits);
    spdlog::info("..............................................................");
    p_chassisDevice = new C_chassisDevice(confif_singchip.config_serial_hardware.serial_device.c_str(), confif_singchip.config_serial_hardware.baud);
    if(p_chassisDevice == NULL){
        spdlog::error("new C_chassisDevice");
        return false;
    }
    return 0;
}



/********************************************************************
 * @author: tianxiaohuahua
 * @date: 23.8
 * @description: processWheelSpeed 一个线程用于处理轮速数据
 * @input: void 
 * @return: void 
*******************************************************************/
// 1/2轮距离，单位：m
#define TIRE_DISTANCE  (0.4f)  // 0.25 
void C_moveApi::loop_processWheelSpeed (void)
{
    int32_t recv = 0;
    std::vector<uint8_t> m_chassisUsartBuffer;
    float leftSpeed = 0;
    float rightSpeed = 0;
    
    int32_t countDisplay = 0; // 所有数据打印过于频繁，每隔20此数据打印一次；数据频率20Hz，20次接近1秒钟

    eCAL::protobuf::CPublisher<MowerProtoPakg::controlMoveOrder> ecalChessPublisher("chessMessage");

    while(1){
        // 接收底盘的数据
        recv = p_chassisDevice->recvUsartData(m_chassisUsartBuffer);
        // 如果接收到数据，处理解析底盘数据
        if(RECV_DONE == recv){ 
            if(m_chassisUsartBuffer[1] == 0x00){
                uint16_t tempLeftSpeed = m_chassisUsartBuffer[3]<<8;
                tempLeftSpeed  = (tempLeftSpeed + m_chassisUsartBuffer[2]) ;
                leftSpeed = tempLeftSpeed * 0.001;
            } else {
                uint16_t tempLeftSpeed = m_chassisUsartBuffer[3]<<8;
                tempLeftSpeed  = (tempLeftSpeed + m_chassisUsartBuffer[2]) ;
                leftSpeed = tempLeftSpeed * -0.001;
            }

            if(m_chassisUsartBuffer[4] == 0x00){
                uint16_t tempRightSpeed = m_chassisUsartBuffer[6]<<8;
                tempRightSpeed  = (tempRightSpeed + m_chassisUsartBuffer[5]) ;
                rightSpeed = tempRightSpeed * 0.001;
            } else {
                uint16_t tempRightSpeed = m_chassisUsartBuffer[6]<<8;
                tempRightSpeed  = (tempRightSpeed + m_chassisUsartBuffer[5]) ;
                rightSpeed = tempRightSpeed * -0.001;
            }
            
            float angleSpeed = (leftSpeed - rightSpeed) / TIRE_DISTANCE;
            float lineSpeed =  (leftSpeed + rightSpeed) / 2;

            countDisplay++;
            if(countDisplay >20){
                spdlog::info("speed L:{} R:{} L:{} A:{}", leftSpeed, rightSpeed, lineSpeed, angleSpeed );
                countDisplay = 0;
            }

            // 发布数据
            MowerProtoPakg::controlMoveOrder chessMessage;// 创建一个protobuf消息对象 发布控制指令
            chessMessage.set_linespeed(lineSpeed);
            chessMessage.set_angularspeed(angleSpeed);
            ecalChessPublisher.Send(chessMessage);
            // for(int i=0; i<9; i++){
            //     printf("%x ", m_chassisUsartBuffer[i]);
            // } printf("\r\n");
        }
    } 
}


/********************************************************************
 * @author: tianxiaohuahua
 * @date: 23.8
 * @description: Callback ecal通信模块的回调函数，会在接收到ecal消息后执行
 * @input: msg 接收到的消息 
 * @return: void 
*******************************************************************/
void C_moveApi::Callback(const MowerProtoPakg::controlMoveOrder& msg) {
    std::vector<uint8_t> sandSerialBuf(10);
    //printf(" move driver line speed:%f ", msg.linespeed() );
    spdlog::info("move driver line speed:{} angle speed:{}", msg.linespeed() ,msg.angularspeed() );

    p_chassisDevice->produceSerialData(msg.linespeed(), msg.angularspeed(), sandSerialBuf);
    p_chassisDevice->sandUsartData(sandSerialBuf);
}

/********************************************************************
 * @author: tianxiaohuahua
 * @date: 23.8
 * @description: runLoopTask 主循环
 * @input: void
 * @return: void 
*******************************************************************/
void C_moveApi::runLoopTask(void)
{
    // 新建一个线程用于转发RTCM数据到串口
    std::thread thread = std::thread(&C_moveApi::loop_processWheelSpeed, this); // 只能这么写，不这么写会报错

    eCAL::protobuf::CSubscriber<MowerProtoPakg::controlMoveOrder> subscriber("ControlMessage");
    subscriber.AddReceiveCallback(std::bind(&Callback, std::placeholders::_2));
    while (eCAL::Ok()) {
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    }
    eCAL::Finalize();// finalize eCAL API
    
    // std::vector<uint8_t> sandSerialBuf(10);
    // float speed = 0;
    // while(1){
    //     std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    //     printf("move test:%.2f\r\n", speed);
    //     speed+=0.02;
    //     if(speed>0.5)speed = 0;
    //     usleep(50000);
    //     p_chassisDevice->produceSerialData(speed, 0, sandSerialBuf);
    //     p_chassisDevice->sandUsartData(sandSerialBuf);
    // }
}

